﻿using System;
using System.Collections.Generic;
using System.Net;
using Microsoft.Xna.Framework;
using PortableTPL;

namespace Babylon.Animations.Skeleton
{
    public class SkinningAnimationPlayer
    {
        private readonly int[] _boneHierarchy;
        private readonly Matrix[] _bindPose;
        private readonly Matrix[] _inverseBindPose;

        private Matrix[] _boneTransforms;
        private Matrix[] _worldTransforms;
        private Matrix[] _skinTransforms;

        public Matrix[] BoneTransforms { get { return _boneTransforms; } }
        public Matrix[] WorldTransforms { get { return _worldTransforms; } }
        public Matrix[] SkinTransforms { get { return _skinTransforms; } }

        private AnimationState _animState;
        public AnimationState AnimState { get { return _animState; } }

        static SkinningAnimationPlayer()
        {
            PortableTPL.TaskScheduler.ProcessorCount = Environment.ProcessorCount;
        }

        private float _currentTime;
        private float _startTime;
        private float _pauseAmount;
        private AnimationClip _currentAnimation;
        private LinkedListNode<SkeletonKeyFrame> _currentKeyFrame;
        
        public void StartAnimation(AnimationClip animationClip,Matrix rootTransform,  float currentTime)
        {
            _currentAnimation = animationClip;
            _currentTime = currentTime;
            _startTime = currentTime;
            _pauseAmount = 0;
            _animState = AnimationState.Running;
            
            if(animationClip.KeyFrames.First.Value.KeyTime != 0.0f)
            {
                var frame = new SkeletonKeyFrame {KeyTime = 0, Transforms = _bindPose};
                _currentAnimation.KeyFrames.AddFirst(frame);
            }

            _currentKeyFrame = animationClip.KeyFrames.First;
            _currentKeyFrame.Value.Transforms.CopyTo(_boneTransforms, 0);
            UpdateWorldTransforms(rootTransform);
            UpdateSkinTransforms();
        }

        public void ResumeAnimation(Matrix rootTransform, float currentTime)
        {
            if(_animState == AnimationState.Paused)
            {
                _animState = AnimationState.Running;
                UpdateAnimation(rootTransform, currentTime);
            }
        }


        public void UpdateAnimation(Matrix rootTransform, float currentTime)
        {
            if(_animState == AnimationState.Running)
            {
                _currentTime = currentTime;
                float elapsed = _currentTime - _startTime - _pauseAmount;
                while (elapsed > _currentAnimation.Duration)
                {
                    _startTime += _currentAnimation.Duration;
                    elapsed = _currentTime - _startTime - _pauseAmount;
                    _currentKeyFrame = _currentAnimation.KeyFrames.First;
                }


                if (_currentKeyFrame.Next == null)
                    _currentKeyFrame = _currentAnimation.KeyFrames.First;

                if (_currentKeyFrame.Next.Value.KeyTime < elapsed)
                {
                    _currentKeyFrame = _currentKeyFrame.Next;
                }


                if (_currentKeyFrame.Next == null)
                    _currentKeyFrame = _currentAnimation.KeyFrames.First;

                var progress = (elapsed - _currentKeyFrame.Value.KeyTime)/
                               (_currentKeyFrame.Next.Value.KeyTime - _currentKeyFrame.Value.KeyTime);

                for (int i = 0; i < _boneTransforms.Length; i++)
                {
                    _boneTransforms[i] =  Matrix.Lerp(
                       _currentKeyFrame.Value.Transforms[i], _currentKeyFrame.Next.Value.Transforms[i], progress);
                }

                UpdateWorldTransforms(rootTransform);
                UpdateSkinTransforms();
            }
            else if(_animState == AnimationState.Paused)
            {
                _pauseAmount += currentTime - _currentTime;
                _currentTime = currentTime;
            }
        }


        public SkinningAnimationPlayer(int[] boneHierarchy, Matrix[] bindPose, Matrix[] inverseBindPose)
        {
            ValidateBoneHierarchy(boneHierarchy);
            _boneHierarchy = boneHierarchy;
            _bindPose = bindPose;
            _inverseBindPose = inverseBindPose;

            if(_boneHierarchy.Length != _bindPose.Length)
                throw new InvalidOperationException("Arrays must be of same length");
            if (_boneHierarchy.Length != _inverseBindPose.Length)
                throw new InvalidOperationException("Arrays must be of same length");

            _boneTransforms = new Matrix[_boneHierarchy.Length];
            _worldTransforms = new Matrix[_boneHierarchy.Length];
            _skinTransforms = new Matrix[_boneHierarchy.Length];

            GoToBindPose();
        }

        public SkinningAnimationPlayer(int[] boneHierarchy, Matrix[] bindPose)
        {

            ValidateBoneHierarchy(boneHierarchy);
            _boneHierarchy = boneHierarchy;
            _bindPose = bindPose;

            if (_boneHierarchy.Length != _bindPose.Length)
                throw new InvalidOperationException("Arrays must be of same length");

            _inverseBindPose = new Matrix[_bindPose.Length];
            for (int i = 0; i < _bindPose.Length; i++)
                _inverseBindPose[i] = Matrix.Invert(_bindPose[i]);


            _boneTransforms = new Matrix[_boneHierarchy.Length];
            _worldTransforms = new Matrix[_boneHierarchy.Length];
            _skinTransforms = new Matrix[_boneHierarchy.Length];
        }

        private void ValidateBoneHierarchy(int[] boneHierarchy)
        {
            if(boneHierarchy == null)
                throw new ArgumentNullException("boneHierarchy");

            for(int i=0;i<boneHierarchy.Length;i++)
            {
                if(boneHierarchy[i]>=i)
                {
                    throw new ArgumentException("boneHierarchy is not correctly sorted.");
                }
            }
        }

        public void GoToBindPose()
        {
            GoToBindPose(Matrix.Identity);
            _animState = AnimationState.Stopped;
        }
        public void GoToBindPose(Matrix rootTransform)
        {
            _bindPose.CopyTo(_boneTransforms,0);
            UpdateWorldTransforms(rootTransform);
            UpdateSkinTransforms();
        }

        private void UpdateSkinTransforms()
        {
            //PortableTPL.Parallel.For(0, _skinTransforms.Length, bone=> {
            //    _skinTransforms[bone] = _inverseBindPose[bone]*_worldTransforms[bone];
            //});

            for(int i=0;i<_skinTransforms.Length;i++)
            {
                _skinTransforms[i] = _inverseBindPose[i] * _worldTransforms[i];
            }
        }

        private void UpdateWorldTransforms(Matrix rootTransform)
        {
            for (int bone = 0; bone < _worldTransforms.Length; bone++)
            {
                int parentBone = _boneHierarchy[bone];
                Matrix parentTransform = rootTransform;
                if(parentBone > -1)
                {
                    parentTransform = _worldTransforms[parentBone];
                }
                _worldTransforms[bone] = _boneTransforms[bone] *
                                             parentTransform;
            }
        }
    }
}
